Implement Mahony filter for attitude estimation - #142
Conversation
bjornhbengtsson
changed the title
|
Yeeeeeees 🔥🔥🔥🔥🔥🔥will review soon |
|
2600 line test 💀 wdym |
|
Yeah I think its nuts too 🐿️ |
|
You'll need a PR in FCF to add mahony.c and the header to the Makefile |
…leration' into feature/mahony-attitude-filter
This reverts commit 4fe0097.
|
For reference, commit |
NArmistead
left a comment
NArmistead
left a comment
There was a problem hiding this comment.
The main changes I'm requesting here are:
- An error status enum for the mahony filter. This is consistent with our standards and will make debugging a little easier since we know which error we're getting.
- Start the filter before the prelaunch terminal so it's active if we're doing dashboard dumps.
I also think it would be more readable if you consolidated some of these functions into one line. I know we don't have a style guide but you can look around our code for some examples (should_log_next_frame is a particularly bad offender for a long line, but probably most readable that way)
All that being said, through all the testing I've done at home this filter does seem to be working and it's great to finally have a step into some better sensor fusion algorithms. This PR should be merged with #138, but I see you already put it in a stack.
| if ( filter == NULL ) | ||
| { | ||
| return false; | ||
| } | ||
|
|
||
| if ( !mahony_quat_is_finite(initial_attitude) ) | ||
| { | ||
| return false; | ||
| } | ||
|
|
||
| if ( !isfinite(proportional_gain) || | ||
| !isfinite(integral_gain) ) | ||
| { | ||
| return false; | ||
| } | ||
|
|
||
| if ( proportional_gain < 0.0f || | ||
| integral_gain < 0.0f ) | ||
| { | ||
| return false; | ||
| } |
There was a problem hiding this comment.
cr: add a status enum for specific error returns
|
|
||
| sensor_reset_velo(); | ||
|
|
||
| (void)mahony_init |
There was a problem hiding this comment.
cr: throw an error if this fails.
sensor init happens during calibration so we can still throw fail-fast errors since this isn't the flight loop yet
| imu_velo_tick = get_us_tick(); | ||
|
|
||
| sensor_reset_velo(); | ||
| QUAT identity = |
There was a problem hiding this comment.
nit: define the identity quat as a constant, probably somewhere in math_sdr.h
|
|
||
| sensor_reset_velo(); | ||
|
|
||
| (void)mahony_init |
There was a problem hiding this comment.
another cr here: we need to start the filter before sensor calibration (which currently calls this function) because it's needed for dashboard dumps during prelaunch
you could probably get away with just running this once in main during startup and then once again during calibration
| delta_time_s = | ||
| (float)imu_tdelta / | ||
| (float)MICROSEC_PER_SEC; |
There was a problem hiding this comment.
this is short enough that it fits on one line
| accel_valid = | ||
| mahony_vector_is_finite(accel_body) && | ||
| isfinite(accel_magnitude) && | ||
| accel_magnitude >= MAHONY_ACCEL_MIN_MAGNITUDE && | ||
| accel_magnitude <= MAHONY_ACCEL_MAX_MAGNITUDE; |
There was a problem hiding this comment.
i think our usual convention is to put the logical operators at the start of each new line (sorry this is hard to format here
accel_valid = mahony_vector_is_finite(accel_body)
&& isfinite(accel_magnitude)
&& etc..
| use_accel = | ||
| get_fc_state() <= FC_STATE_LAUNCH_DETECT; | ||
|
|
||
| (void)mahony_update_imu |
There was a problem hiding this comment.
cr: we probably want to check the status here too. in this case, we could at least use a debug_assert to catch if anything weird happens. worst case we could also reinitialize the filter if something completely breaks
| @@ -0,0 +1,106 @@ | |||
| ################################################################ | |||
| # | |||
| # math_sdr unit tests (based on gcc) | |||
|
|
||
| for ( index = 0; index < 2000; index++ ) | ||
| { | ||
| TEST_ASSERT_TRUE |
There was a problem hiding this comment.
if you don't want to do 2000 assertions in this loop and you make a status enum where success is 0 in the enum (our convention), and there are no other acceptable states (i.e. the rest of the returns are some form of error, which is the case here), you could |= the status return each time so that you only need to assert once at the end.
that would also make the results file a little more navigable and prevent you from having 20000 (!!) asserts
There was a problem hiding this comment.
this is a really long file to go through every test, so im gonna leave this as is for now and come back to it once you make changes to everything else. these are some pretty nice and thorough tests though and good to verify with determinate tests that the filter does what it should
|
Another thing I noticed is that enabling integral correction with the accelerometer will affect the heading (global z axis) and create drift, which it shouldn't do because acceleration doesn't tell us anything about that. Presumably mag fusion would fix that? |
|
new mahony unit test result |
|
I addressed the remaining Mahony review comments: Added MAHONY_STATUS enum return codes and updated the related tests. Validation: Mahony tests pass, math_sdr tests pass, the fsm_appa test passes, and both debug and release Rev2 firmware builds pass. |
|
I left the style changes to you, seems more effective that way. |
2 participants
Description
Implements the Mahony attitude filter for quaternion based attitude estimation.
The filter maintains a world-to-body attitude quaternion using body frame gyroscope propagation with proportional and integral accelerometer feedback.
Made with assistance of ChatGPT 5.6.
Changes include:
Issue Link
Depends on #138
#138
Closes #141
#141
Testing
Mahony unit test results:
-------------Test Complete--------------
Passes: 140
Fails: 0
Result: PASS
Math test results:
-------------Test Complete--------------
Passes: 26
Fails: 0
Result: PASS
Other
Accelerometer correction is only applied when:
When accelerometer feedback is disabled or rejected, the filter continues using gyroscope only propagation.
The current accelerometer thresholds, proportional gain, integral gain, and integral limit are initial software values. Final tuning should be performed using hardware characterization, vibration testing, and flight data.
Deferred work includes:
Reviewer Checklist
Standards
Error Handling
Memory
Performance